
#include <iostream>
#include <fstream>

//ROS stuff
#include "ros/ros.h"

//Matrix library
#include <newmat/newmatap.h>
#include <newmat/newmatio.h>
#include <math.h>

#include "MULTILOC/DetectedObjectsMsg.h"
#include "MULTILOC/DynamicObjectMsg.h"

using namespace NEWMAT;

class Crossloc
{
public:
  Crossloc();

  ros::NodeHandle n;
  ros::Subscriber ObjectList_sub;
  void ObjectList_Callback(const MULTILOC::DetectedObjectsMsg::ConstPtr& msg);
  void RobPosCallback();
  void Position_EKF();
  NEWMAT::Matrix RobPos;
  NEWMAT::Matrix RobPPos;
  NEWMAT::Matrix RobPhyChar;
  NEWMAT::Matrix RobPPhyChar;


private:
  float BhaDist(NEWMAT::Matrix,NEWMAT::Matrix,NEWMAT::Matrix,NEWMAT::Matrix);
  float PhyCharMinDistTol;
};
